import serial
from time import sleep
from threading import Thread
debug=False

GarbagePostion={"Recyclable":[180,180],\
                "Harmful":[15,180],\
                "Kitchen":[180,60],\
                "Other":[15,60]}

ser=serial.Serial("/dev/ttyUSB0",115200)

class Control:
    
    def __init__(self,ser1=ser):
        self.ser=ser1
        sleep(2)
        self.PlatBalence()
        
    '''
        { xx xx xx xx xx xx}
        {
            命令选择
            数据1高8位
            数据1低8位
            数据2高8位
            数据2低8位
        }
    '''
    def PlatMovement(self,horizontal,vertical):
        # 角度0~256
        data=[0x7b,0x01,0xff,0xff,0x00,0x00,0x7d,0x0a]
        data[2]=horizontal
        data[3]=vertical
        self.ser.write(bytes(data));
    
    
    def PlatBalence(self):
        self.PlatMovement(90,120)
    
    def TurnToGarbage(self,garbage):
        try:
            position=GarbagePostion[garbage]
        except:
            print(garbage,"dosen't exist !")
            return
        self.PlatMovement(position[0],position[1])
        sleep(4)
        self.PlatBalence()
        sleep(2)
    
    # 吸盘xy位置移动
    def SuckerMove(self,x,y):
        data=[0x7b,0x02,0xff,0xff,0xff,0xff,0x7d,0x0a]
        data[2]=x>>8
        data[3]=x&0xff
        data[4]=y>>8
        data[5]=y&0xff        
        self.ser.write(bytes(data));
    #吸盘垂直移动
    def SuckerMoveToHeight(self,height):
        # 角度0~256
        data=[0x7b,0x03,0xff,0x00,0x00,0x00,0x7d,0x0a]
        data[2]=height
        self.ser.write(bytes(data));
    # 吸取
    def Suck(self):
        # 1为吸取 0为松开
        data=[0x7b,0x04,0xff,0x00,0x00,0x00,0x7d,0x0a]
        data[2]=1
        self.ser.write(bytes(data));
    #放下
    def Unsuck(self):
        # 1为吸取 0为松开
        data=[0x7b,0x04,0xff,0x00,0x00,0x00,0x7d,0x0a]
        data[2]=0
        self.ser.write(bytes(data));
    # 压缩
    def Compress(self):
        # 1为压缩 0为解压缩
        data=[0x7b,0x05,0xff,0x00,0x00,0x00,0x7d,0x0a]
        data[2]=1
        self.ser.write(bytes(data));
    # 停止压缩
    def Uncompress(self):
        # 1为压缩 0为解压缩
        data=[0x7b,0x05,0xff,0x00,0x00,0x00,0x7d,0x0a]
        data[2]=0
        self.ser.write(bytes(data));
    
    def __del__(self):
        self.ser.close()

class FullDetect:
    def __init__(self,ser1=ser):
        self.ser=ser1
        self.run=False
        self.FullStatus=[0,0,0,0]
        sleep(2)

    def Start(self):
        self.run = True

    def Stop(self):
        self.run = False
    def ReadFullStatus(self):
        while not self.ser.in_waiting:
            if debug:
                print(str("in_waiting") + "data is waiting...")
        recData = []
        for i in range(5):
            while not self.ser.in_waiting:
                if debug:
                    print("Waiting For Data...")
            dat = self.ser.read()
            recData.append(dat)

        Data = []
        Data.append(int.from_bytes(recData[0], byteorder='big', signed=True) )
        Data.append(int.from_bytes(recData[1], byteorder='big', signed=True) )
        Data.append(int.from_bytes(recData[2], byteorder='big', signed=True) )
        Data.append(int.from_bytes(recData[3], byteorder='big', signed=True) )
        return Data

    def RecData(self):
        try:
            while self.run: 
                if self.ser.readable():
                    StartFlag = self.ser.read()
                    if StartFlag == b'\x7b':
                        while not self.ser.in_waiting:
                            if debug:
                                print("Waiting For Data")
                        self.FullStatus = self.ReadFullStatus()
                        if debug:
                            print(self.FullStatus)
                        self.ser.flushInput()
        except KeyboardInterrupt:
            self.self.Stop()
            
    def GetFullStatus(self):
        return self.FullStatus
    def __del__(self):
        self.Stop()

if __name__ == "__main__":
    fd=FullDetect()
    fd.Start()
    t1=Thread(target=fd.RecData)
    t1.start()
    while True:
        print(fd.GetFullStatus())
        sleep(1)

        

